﻿using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using NetduinoHappyHour.Util;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;

namespace NetduinoHappyHour.Hardware
{
    /// <summary>
    /// Basic drive command to be executed on the robot
    /// Written by: Salvador Richter
    /// Appache License Version 2.0 
    /// </summary>
    public class DriveCommand : IDriveCommand, IConstructors
    {
        public enum NextStep
        {
            Left,
            Right,
            Both
        };

        // lock object to avoid re entry on ExecuteDriveCommand in multithreaded environment
        private object _lock = new object();

        public int StepDuration { get; private set; }
        // these are required if multithreaded execution of DriveCommand in Gateway is implemented
        //public int StepDurationL { get; private set; }
        //public int StepDurationR { get; private set; }

        public int StepsToRunL { get; private set; }
        public int StepsToRunR { get; private set; }

        public int StepsAlreadyRanL { get; private set; }
        public int StepsAlreadyRanR { get; private set; }

        public bool DirectionForwardL { get; private set; }
        public bool DirectionForwardR { get; private set; }
        
        /// <summary>
        /// Might be used to interupt execution of DriveCommand.
        /// DriveCommand is executed as long as it is valid
        /// </summary>
        public bool IsValid { get; set; }

        /// <summary>
        /// DriveComand is endless executed if set to true
        /// Default: false
        /// </summary>
        public bool SelfRepeating { get; set; }

        /// <summary>Creates a DriveCommand using param in string representation</summary>
        ///
        /// <param name="stepsL">number of steps to be run by left engine, for movement per step see SystemConfig, on negative Value DirectionForwardL is set to false</param>
        /// <param name="stepsR">number of steps to be run by right engine, for movement per step see SystemConfig, on negative Value DirectionForwardR is set to false</param>
        /// <param name="stepDuration">
        ///           step duration of faster wheel, step duration of slower wheel
        ///           gets adjusted, if zero default speed is used
        /// </param>
        public DriveCommand(String stepsL, String stepsR, String stepDuration):this(int.Parse(stepsL),int.Parse(stepsR),int.Parse(stepDuration))
        {
        }

        /// <summary>Creates a DriveCommand, with no drive information. valuable for EnineSleepMode()</summary>
        ///
        /// <param name="stepsL">number of steps to be run by left engine, for movement per step see SystemConfig, on negative Value DirectionForwardL is set to false</param>
        /// <param name="stepsR">number of steps to be run by right engine, for movement per step see SystemConfig, on negative Value DirectionForwardR is set to false</param>
        /// <param name="stepDuration">
        ///           step duration of faster wheel, step duration of slower wheel
        ///           gets adjusted, if zero default speed is used
        /// </param>
        public DriveCommand()
        {
        }

        /// <summary>Creates a DriveCommand</summary>
        ///
        /// <param name="stepsL">number of steps to be run by left engine, for movement per step see SystemConfig, on negative Value DirectionForwardL is set to false</param>
        /// <param name="stepsR">number of steps to be run by right engine, for movement per step see SystemConfig, on negative Value DirectionForwardR is set to false</param>
        /// <param name="stepDuration">
        ///           step duration of faster wheel, step duration of slower wheel
        ///           gets adjusted, if zero default speed is used
        /// </param>
        public DriveCommand(int stepsL, int stepsR, int stepDuration)
        {
            StepsAlreadyRanL = 0;
            StepsAlreadyRanR = 0;
            IsValid = true;
            SelfRepeating = false;
            DirectionForwardL = stepsL >= 0;
            DirectionForwardR = stepsR >= 0;
            StepsToRunL = System.Math.Abs(stepsL);
            StepsToRunR = System.Math.Abs(stepsR);

            // avoid zero Value
            if (stepDuration == 0)
            {
                StepDuration = SystemConfig.Instance().DelayStepper;
            }
            else
            {
                StepDuration = System.Math.Abs(stepDuration);
            }
            /* not needed in single thread implementation
            StepDurationL = StepDuration;
            StepDurationR = StepDuration;

            // special handling required if one stepper command is zero
            if (StepsToRunL > StepsToRunR)
            {
                if (StepsToRunL % StepsToRunR != 0)
                {
                    throw new ConstraintException();
                }
                StepDurationR = StepsToRunL / StepsToRunR * StepDuration;
            }
            if (StepsToRunR > StepsToRunL)
            {
                if (StepsToRunR % StepsToRunL != 0)
                {
                    throw new ConstraintException();
                }
                StepDurationL = StepsToRunR / StepsToRunL * StepDuration;
            }
            */
        }

        public String[] GetConstructors()
        {
            return new String[] { "()", "(int stepsL, int stepsR, int stepDuration)" };
        }

        /// <summary>
        /// Executes a DriveCommand. Each step if postponed by a wait cycle, so no special handling for engines rotation required
        /// Uses GPIO PIN D10 to D13
        /// </summary>
        /// <param name="dCommand">
        /// The drive command to be executed
        /// </param>
        public void Execute()
        {
            // lock is required because of multithreaded environment to avoid system conflicts on single ressource.
            //ToDo remove unused lock
            //lock (_lock)
            {
                DriveCommand.NextStep next;
                // TODO switchen und testen
                bool lowValue = false;
                bool highValue = !lowValue;
                StepperSleep reactivateSleepMode = null;
                OutputPort onBoardLed = new OutputPort(Pins.ONBOARD_LED, lowValue);
                // clock left
                OutputPort stepperL = new OutputPort(SystemConfig.Instance().StepperClockLeft, lowValue);
                // direction left
                OutputPort directionL = new OutputPort(SystemConfig.Instance().StepperDirectionLeft, DirectionForwardL);
                // clock right
                OutputPort stepperR = new OutputPort(SystemConfig.Instance().StepperClockRight, lowValue);
                // direction right
                OutputPort directionR = new OutputPort(SystemConfig.Instance().StepperDirectionRight, DirectionForwardR);

                int stepDuration = StepDuration / 2;
                int stepDurationEven = stepDuration % 2;

                // activate stepper if required, store sleep Mode
                if (StepperSleep.IsSleepMode)
                {
                    reactivateSleepMode = new StepperSleep(false);
                    reactivateSleepMode.Execute();
                }
                while (IsValid && NextStepAllowed())
                {
                    next = GetStepToExecute();
                    if (SelfRepeating && !NextStepAllowed())
                    {
                        Reset();
                    }
                    // toggle the Value to create a clock, driver reacts on rising edge only
                    System.Threading.Thread.Sleep(stepDuration + stepDurationEven);
                    if (next == DriveCommand.NextStep.Left || next == DriveCommand.NextStep.Both)
                    {
                        stepperL.Write(highValue);
                    }
                    if (next == DriveCommand.NextStep.Right || next == DriveCommand.NextStep.Both)
                    {
                        stepperR.Write(highValue);
                    }
                    onBoardLed.Write(highValue);
                    System.Threading.Thread.Sleep(stepDuration);
                    if (next == DriveCommand.NextStep.Left || next == DriveCommand.NextStep.Both)
                    {
                        stepperL.Write(lowValue);
                    }
                    if (next == DriveCommand.NextStep.Right || next == DriveCommand.NextStep.Both)
                    {
                        stepperR.Write(lowValue);
                    }
                    onBoardLed.Write(lowValue);
                }

                // deactivate steppers if they slept before
                if (reactivateSleepMode != null)
                {
                    StepperSleep.IsSleepMode = true;
                    reactivateSleepMode.Execute();
                }

                onBoardLed.Dispose();
                stepperL.Dispose();
                stepperR.Dispose();
                directionL.Dispose();
                directionR.Dispose();
            }
        }

        /// <summary>
        /// Sets StepsAlreadyRanX to 0
        /// Sets IsValid to true
        /// </summary>
        public void Reset()
        {
            StepsAlreadyRanL = 0;
            StepsAlreadyRanR = 0;
            IsValid = true;
        }

        /// <summary>
        /// Determines weather execution of next step is allowed or not
        /// </summary>
        /// <returns></returns>
        public bool NextStepAllowed()
        {
            if (StepsToRunL >= StepsToRunR)
            {
                return StepsToRunL > StepsAlreadyRanL;
            }
            else
            {
                return StepsToRunR > StepsAlreadyRanR;
            }
        }

        /// <summary>
        /// Returns wich step is to be executed, Left Right or Both
        /// Increments the number of already ran steps
        /// </summary>
        /// <returns></returns>
        public NextStep GetStepToExecute()
        {
            int factor = 1;
            NextStep next = NextStep.Both;
            // TODO return valid step to Execute
            if (StepsToRunL == StepsToRunR)
            {
                StepsAlreadyRanL++;
                StepsAlreadyRanR++;
                next = NextStep.Both;
            }
            else if (StepsToRunL == 0)
            {
                StepsAlreadyRanR++;
                next = NextStep.Right;
            }
            else if (StepsToRunR == 0)
            {
                StepsAlreadyRanL++;
                next = NextStep.Left;
            }
            else if (StepsToRunL > StepsToRunR)
            {
                factor = StepsToRunL / StepsToRunR;
                StepsAlreadyRanL++;
                if (StepsAlreadyRanL % factor == 0)
                {
                    StepsAlreadyRanR++;
                    next = NextStep.Both;
                }
                else
                {
                    next = NextStep.Left;
                }
            }
            else if (StepsToRunR > StepsToRunL)
            {
                factor = StepsToRunR / StepsToRunL;
                StepsAlreadyRanR++;
                if (StepsAlreadyRanR % factor == 0)
                {
                    StepsAlreadyRanL++;
                    next = NextStep.Both;
                }
                else
                {
                    next = NextStep.Right;
                }
            }
            return next;
        }
    }
}
